#include "dji_linux_helpers.hpp"
#include "dji_linker.hpp"
#include "GSLAM/core/GSLAM.h"
#include "RTMapperNetInterface.h"

using namespace GSLAM;
using namespace sv;


class GimbalControl{
public:
    GimbalControl(Vehicle* vehicle)
        : vehicle(vehicle){
        initialize();
    }
    ~GimbalControl(){}

    void initialize(){
        int functionTimeout = 1;
        // Obtain Control Authority
        vehicle->control->obtainCtrlAuthority(functionTimeout);
    }

    void set_gimbal_position(int pitchRate, int yawRate, int rollrate){

        DJI::OSDK::Gimbal::SpeedData gimbalSpeed;

        gimbalSpeed.roll     = 0;
        gimbalSpeed.pitch    = pitchRate;
        gimbalSpeed.yaw      = yawRate;
        gimbalSpeed.gimbal_control_authority = 1;
        vehicle->gimbal->setSpeed(&gimbalSpeed);
    }


public:
    Vehicle* vehicle;


};



REGISTER_SVAR_MODULE(GimbalControl){
    Class<GimbalControl>("GimbalControl")
            .unique_construct<Vehicle*>()
            .def("set_gimbal_position",&GimbalControl::set_gimbal_position);

}

